#include "main.h"
#include "adc.h"
#include "timer.h"
#include "hardware.h"
#include <math.h>
// 使用ADC读取一个通道的数据（阻塞方式）
static uint16_t adc_read(ADC_TypeDef *ADCx, uint32_t channel)
{
    LL_ADC_REG_SetSequencerRanks(ADCx, LL_ADC_REG_RANK_1, channel);
    LL_ADC_REG_StartConversion(ADCx);
    while (!LL_ADC_IsActiveFlag_EOS(ADCx));
    uint16_t result = LL_ADC_REG_ReadConversionData12(ADCx);
    LL_ADC_ClearFlag_EOS(ADCx);

    return result;
}

// 等待一个ADC通道稳定
static uint16_t wait_and_get_adc_block(ADC_TypeDef *ADCx, uint32_t channel, float threshold)
{
    const uint32_t timeout_ms = 1000; // 1秒超时
    const uint32_t sample_interval_ms = 10; // 采样间隔10ms
    uint32_t start_time = get_absolute_time() / 1000;
    uint32_t last_voltage = 0;
    bool first_reading = true;
    while (true) {
        // 读取ADC值
        uint32_t current_voltage = 0;
        for (int i = 0; i < 1024; i++) {
            current_voltage += adc_read(ADCx, channel);
        }
        current_voltage /= 1024;
        if (first_reading) {
            first_reading = false;
            last_voltage = current_voltage;
            sleep_ms(sample_interval_ms);
            continue;
        }
        // 检查电压是否稳定
        float deviation = fabsf((float)current_voltage - (float)last_voltage) / last_voltage;
        if (deviation < threshold) {
            return (uint16_t)current_voltage;
        }
        last_voltage = current_voltage;
        // 检查超时
        if (get_absolute_time() / 1000 - start_time >= timeout_ms) {
            return (uint16_t)current_voltage; // 超时返回最后一次读数
        }
        sleep_ms(sample_interval_ms);
    }
}

// 等待单片机的3.3V供电稳定
// 返回单片机Vref电压，单位V
// 执行过程中不要使用ADC的注入组，可能会打断采样保持的步骤，影响精度
float wait_vref_3p3v(void){
    // 连续采样Vrefint，直到最后两次采集的电压误差＜0.2%(对应采样原始值＜3)，或者超时
    float result_vrefint = wait_and_get_adc_block(ADC1, LL_ADC_CHANNEL_VREFINT, 0.002f);
    // Vref = 3.0V时VrefInt的ADC采样原始值（校准值）
    float result_vrefint_3p0v = (*VREFINT_CAL_ADDR);    
    // 根据公式：VREF+ = VREF+_Charac × VREFINT_CAL / VREFINT_DATA
    return 3.0f * result_vrefint_3p0v / result_vrefint;
}

// 等待单片机的24V供电稳定
// 返回电机供电电压VM，单位V
float wait_vm_24v(float vref_voltage){
    // 连续采样Vrefint，直到最后两次采集的电压误差小于0.5%，或者超时
    float vm_adc = wait_and_get_adc_block(ADC2, LL_ADC_CHANNEL_1, 0.005f);
    return vref_voltage * ( vm_adc / ADC_RESOLUTION ) / VM_FACTOR;
}

// 换算单片机内部温度（单位：摄氏度）
// 电机转动时，波动较大（±2℃左右），可能是因为内部温度传感器输出电阻很高，容易受到其他通道的干扰
static float calculate_mcu_temp(uint16_t adc_val, float vref_voltage){
    // 读取ADC值，并且换算为Vref = 3.0V时从采集结果
    float TS_DATA = adc_val  * (vref_voltage / 3.0f);

    // TS ADC raw data acquired at a temperature of 30 °C (± 5 °C), VDDA = VREF+ = 3.0 V (± 10 mV)
    float TS_CAL1 = (*TEMPSENSOR_CAL1_ADDR);
    float TS_CAL1_TEMP = 30.0f;

    // TS ADC raw data acquired at a temperature of 130 °C (± 5 °C), VDDA = VREF+ = 3.0 V (± 10 mV)
    // LL库中的 #define TEMPSENSOR_CAL2_TEMP (110L) 是错的，以规格书 DS12589 Rev 6 为准
    float TS_CAL2 = (*TEMPSENSOR_CAL2_ADDR);
    float TS_CAL2_TEMP = 130.0f;

    return (TS_CAL2_TEMP - TS_CAL1_TEMP) / (TS_CAL2 - TS_CAL1) * (TS_DATA - TS_CAL1) + TS_CAL1_TEMP;
}

// 近似计算logf（以e为底，也就是数学上的ln函数），用于温度计算时，对计算结果的影响小于0.01℃
// 不含错误处理，请传入正确的数据
static float fast_logf_poly(float x) {
    typedef union {
        float f;
        uint32_t u;
    } float_uint;
    float_uint fu = {.f = x};
    
    // 提取指数和尾数
    int32_t exp = ((fu.u >> 23) & 0xFF) - 127;
    float m = 1.0f + (float)(fu.u & 0x7FFFFF) * (1.0f / 8388608.0f); // 1/(2^23)
    
    // z = (m-1)/(m+1)
    float z = (m - 1.0f) / (m + 1.0f);
    float z2 = z * z;
    
    // 5阶多项式系数
    float log_m = z * (2.0f + z2 * (0.6666666f + z2 * 0.4000000f));
    
    return 0.69314718f * exp + log_m; // log(2) ≈ 0.69314718
}

// 换算NTC温度
static float calculate_ntc_temp(uint16_t result_ntc){
    // 计算NTC温度

    // 根据Steinhart-Hart方程 1/T = A + B*ln(R) + C*(ln(R))**3 计算
    // 相对于常规的B值方式，精度更高
    // Steinhart-Hart系数（详见python_tools/verify_ntc.py）  
    // 对热敏电阻规格书中的R-T表拟合的最大温度误差0.1372℃
    #define SH_A 1.1218199445e-03f
    #define SH_B 2.3582428380e-04f  
    #define SH_C 7.7346110381e-08f
    
    // 直接使用ADC原始值计算NTC电阻（vref被约掉）
    float ntc_resistance;
    if (result_ntc > 0) {
        ntc_resistance = NTC_R_REF / (ADC_RESOLUTION / (float)result_ntc - 1.0f);
    } else {
        // ADC值=0表示NTC处于极端高温状态或测量电路异常，返回一个较大的温度数据，触发过热保护。
        return 500.0f;
    }
    
    // 使用Steinhart-Hart方程计算NTC温度
    float lnR = fast_logf_poly(ntc_resistance);              // ln(R)
    float invT = SH_A + SH_B * lnR + SH_C * lnR * lnR * lnR; // 1/T = A + B*lnR + C*(lnR)^3
    float tempK = 1.0f / invT;                               // T(K) = 1 / (1/T)
    float ntc_temperature = tempK - 273.15f;                 // T(°C) = T(K) - 273.15
    // printf_("NTC Resistance = %.2f ohms, NTC Temperature = %.2f C\n", ntc_resistance, ntc_temperature);
    return ntc_temperature;
}

void get_temperature(float *mcu_temp, float *ntc_temp, float vref_voltage){
    // 分别配置ADC1和ADC2
    LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_TEMPSENSOR_ADC1);
    LL_ADC_REG_SetSequencerRanks(ADC2, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_5);
    // 开始转换
    LL_ADC_REG_StartConversion(ADC1);
    LL_ADC_REG_StartConversion(ADC2);
    // 等待转换完成后读取
    while (!LL_ADC_IsActiveFlag_EOS(ADC1));
    uint16_t mcu_temperature_adc = LL_ADC_REG_ReadConversionData12(ADC1);
    LL_ADC_ClearFlag_EOS(ADC1);
    while (!LL_ADC_IsActiveFlag_EOS(ADC2));
    uint16_t ntc_temperature_adc = LL_ADC_REG_ReadConversionData12(ADC2);
    LL_ADC_ClearFlag_EOS(ADC2);
    // 换算为摄氏度
    float mcu_temperature = calculate_mcu_temp(mcu_temperature_adc, vref_voltage);
    float ntc_temperature = calculate_ntc_temp(ntc_temperature_adc);
    // 滤波
    const float w =  0.2f;
    *mcu_temp = (1.0f - w) * (*mcu_temp) + w * mcu_temperature;
    *ntc_temp = (1.0f - w) * (*ntc_temp) + w * ntc_temperature;
}